#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <boost/filesystem.hpp>
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <cstdlib>
#include <iostream>
#include <string.h>
#include <sstream>
#include <termios.h>
#include <typeinfo>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <rosbag/bag.h>
#include <rospack/rospack.h>
#include <boost/filesystem.hpp>


static size_t cnt = 0;
char c = EOF;
bool flag = true;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> policy;

auto pkg_path = boost::filesystem::path(ros::package::getPath("bag_dealer")) / "data";
static size_t index_synced = 1;

void sync_cb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::PointCloud2ConstPtr& pc_msg)
{
    //save synchronized image and point cloud here
    if (c == 'c' && flag)
    {
        auto img_path = pkg_path / "image";
        auto pc_path = pkg_path / "point_cloud";

        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg, image_msg->encoding);
        auto img_cv = cv_ptr->image;
        cv::cvtColor(img_cv, img_cv, CV_RGB2BGR);
        cv::imwrite((img_path / std::to_string(index_synced)).string() + ".jpg", img_cv);

        pcl::PCDWriter pc_writer;
        pcl::PointCloud<pcl::PointXYZI> cloud;
        pcl::fromROSMsg(*pc_msg, cloud);
        pc_writer.write((pc_path / std::to_string(index_synced)).string() + ".pcd", cloud);

        ++index_synced;
        flag = false;
    }
}

//int getch()
//{
//    static struct termios oldt, newt;
//    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
//    newt = oldt;
//    newt.c_lflag &= ~(ICANON);                 // disable buffering
//    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings
//    int c = getchar();  // read character (non-blocking)
//    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
//    return c;
//}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "data_capturer");
    ros::NodeHandle nh_;
    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh_, argv[1], 10);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh_, argv[2], 10);
    message_filters::Synchronizer<policy> sync(policy(10), image_sub, pc_sub);

    sync.registerCallback(boost::bind(&sync_cb, _1, _2));

    while(ros::ok())
    {
        ROS_INFO_STREAM("c to capture a pair of image and point cloud" << "\ne to exit");
        c = getchar();
        if (c == 'e')
            ros::shutdown();
        if (c == '\n')
            flag = true;
        if (c == 'r')
            --index_synced;
        ros::spinOnce();
    }

    return 0;
}